/* Copyright (c) 2022-2023 Qualcomm Technologies, Inc.
 * All Rights Reserved.
 * Confidential and Proprietary - Qualcomm Technologies, Inc.
 */

#ifndef _TRACKER_H_
#define _TRACKER_H_

#include <mutex>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <memory>
#include <chrono>
#include <numeric>
#include <sys/stat.h>
#include <future>

#include <net.h>
#include "Camera_D435i.h"

#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/float32.hpp>  // 用于发布角度和距离
#include <geometry_msgs/msg/pose.hpp>  // 如果需要发布目标的位姿信息
#include <geometry_msgs/msg/vector3.hpp>

#define TRACKING_TYPE_CV 1
#define TRACKING_TYPE_LIDAR 2
#define TRACKING_TYPE_DEFAULT TRACKING_TYPE_LIDAR

// ncnn model parameters
#define NCNN_INPUT_NAME ("input.1")
#define NCNN_OUTPUT_NAME    ("758")
#define NCNN_MODEL_INPUT_WIDTH  (352)      
#define NCNN_MODEL_INPUT_HEIGTH (352)
#define NCNN_MODEL_THRESH   (0.7)

class TargetBox
{
public:
    int GetWidth() { return (x2 - x1); };
    int GetHeight() { return (y2 - y1); };

    TargetBox() {}
    TargetBox(int _x1, int _y1, int _x2, int _y2, int _category = 0, float _score = 0): \
        x1(_x1), y1(_y1), x2(_x2), y2(_y2), category(_category), score(_score) {}
        
    int x1;
    int y1;
    int x2;
    int y2;

    int category;
    float score;

    float area() { return GetWidth() * GetHeight(); };
};

class MyTracker
{
public:
    MyTracker();
    MyTracker(int type, std::string model_param_file, std::string model_bin_file, std::string label_file);
    ~MyTracker();
    MyTracker(const MyTracker & ) = delete;

    bool trackingOnce(std::vector<TargetBox> &target_boxes);
    void getTargetPose(const TargetBox &box, float &theta, float &d);
    void stopTracking();
    void getImage_thread(bool &running);
    void getImageOnce(void);
    void camLock();
    void camUnlock();

    // 发布目标信息的 ROS 发布者
    rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr target_info_pub;
    // 发布目标信息
    void publishTargetInfo(const std::vector<TargetBox> &target_boxes);

private:
    void initCvTracker(void);
    bool cvObjectDetection(const cv::Mat &img, std::vector<TargetBox> &target_boxes);

    void getTargetDistance(cv::Rect &bbox, float &horizon_dist_to_target);

    /* params */
    int tracking_type;

    /* camera */
    Camera_D435i cam;
    /* camera input data */
    uint16_t depth[IMAGE_WIDTH * IMAGE_HEIGHT];
    cv::Mat frame;
    std::mutex camMutex;
    ncnn::Net net;
    // cncc model file
    std::string model_param_file;
    std::string model_bin_file;
    std::string label_file;
    float thresh;
    int input_width;
    int input_height;
    std::vector<std::string> labels;
    std::vector<int> labelsIndexToDetect;
    uint8_t *data_int8;
    float *data_f32;
    geometry_msgs::msg::Vector3 msg;
    float minDis;
    float minDisAngle;
    std::vector<TargetBox> target_boxes;
};

#endif
